#include "ExtractLines.h"
#include "ContourCornerFinder.h"
#include "MiscFunctions.h"

#include <opencv2/imgproc/imgproc_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <stdio.h>

using namespace cv;
using namespace std;
using namespace idch;

ExtractLines::ExtractLines( int minLinePoints , double lineFitTol , bool findSmallSegments )
{
    this->lineFitTol = lineFitTol*lineFitTol;
    this->minLinePoints = minLinePoints;
    this->findSmallSegments = findSmallSegments;
}

ExtractLines::~ExtractLines() {

}

void ExtractLines::extract( const std::vector<cv::Point> &contour ,
                            const std::vector<ContourCorner> &corners ) {
    lines.clear();
    if( corners.size() <= 1 )
        return;

    // find lines between the corner points in the contour
    std::vector<cv::Point> list;
    for( size_t i = 1; i < corners.size(); ++i ) {
//        printf("corner index %d\n",corners.at(i).index);
        findLines(contour,corners.at(i-1).index, corners.at(i).index,list );
    }
    findLines(contour,corners.at(corners.size()-1).index, corners.at(0).index,list );

//    printf("Found lines total %d\n",(int)lines.size());
}



const std::vector<LineSegmentF>& ExtractLines::getFound() const {
    return lines;
}

void ExtractLines::configure( double lineFitTol ) {
    this->lineFitTol = lineFitTol*lineFitTol;
}

void ExtractLines::findLines( std::vector<cv::Point>& list ) {
//    printf("find line 1 param %d\n",(int)list.size());
    // find best fit line
    LineSlopeF line = fitLine(list);

//    printf(" found line pt( %f  , %f ) slope %f  %f\n",line.x0,line.y0,line.slopeX,line.slopeY);
    // find the farthest point from the line
    size_t worstIndex = 0;
    cv::Point pt = list.at(0);
    float worstDist = lineDistance2<float>(line.x0,line.y0,line.slopeX,line.slopeY,pt.x,pt.y);

    for( size_t i = 1; i < list.size(); ++i ) {
        pt = list.at(i);
        float a = lineDistance2<float>(line.x0,line.y0,line.slopeX,line.slopeY,pt.x,pt.y);

        if( a > worstDist ) {
            cv::Point b = list.at(i-1);
//            printf(" worse %f idx = %d prev( %d , %d ) curr( %d , %d )\n",a,(int)i,b.x,b.y,pt.x,pt.y);
            worstDist = a;
            worstIndex = i;
        }
    }

//    printf("-- worst distance %f index %d\n",sqrt(worstDist),(int)worstIndex);

    // see if its a good line
    if( worstDist < lineFitTol ) {
//        printf("SAVING LINE!\n");
        // save the line
        saveLine(line,list.at(0),list.at(list.size()-1));
    } else if( findSmallSegments ){
        // split the line at the point farthest away
        std::vector<cv::Point> brokenLine;

        if( worstIndex == 0 ) {
            findLines( list , 1 , list.size() , brokenLine);
        } else if( worstIndex == list.size()-1 ) {
            findLines( list , 0 , list.size()-1 , brokenLine);
        } else {
            findLines( list , 0 , worstIndex , brokenLine);
            findLines( list , worstIndex , list.size() , brokenLine);
        }
    }
}

void ExtractLines::findLines( const std::vector<cv::Point>& origList ,
                              int begin , int end  ,
                              std::vector<cv::Point>& list ) {
//    printf("find line many param origList.size = %d  b = %d e = %d\n",(int)origList.size(),begin,end);
    putIntoList(origList,begin,end,list);

//    printf("  list size = %d\n",(int)list.size());
    if( list.size() >= minLinePoints ) {
        findLines( list );
    }
}

LineSlopeF ExtractLines::fitLine( std::vector<cv::Point>& list ) {
//    printf("fitLine() list.size = %d\n",(int)list.size());

    Vec4f line;
    cv::fitLine(cv::Mat(list),line,CV_DIST_L2,0,0.01,0.01);

    LineSlopeF ret;

    ret.x0 = line[2];
    ret.y0 = line[3];
    ret.slopeX = line[0];
    ret.slopeY = line[1];

    return ret;
}

void ExtractLines::putIntoList( const std::vector<cv::Point> &contour ,
                                int start , int stop ,
                                std::vector<cv::Point> &list )
{
    list.clear();

    if( start < stop ) {
        for( int i = start; i < stop; i++ )
            list.push_back( contour.at(i) );
    } else {
        int N = contour.size();

        for( int i = start; i < N; i++ )
            list.push_back( contour.at(i) );
        for( int i = 0; i < stop; i++ )
            list.push_back( contour.at(i) );
    }
}

void ExtractLines::saveLine( LineSlopeF line , cv::Point a , cv::Point b )
{
    // project the points into the line
    LineSegmentF ls;

    closestPoint<float>(line.x0,line.y0,line.slopeX,line.slopeY,a.x,a.y,ls.x0,ls.y0);
    closestPoint<float>(line.x0,line.y0,line.slopeX,line.slopeY,b.x,b.y,ls.x1,ls.y1);

    lines.push_back(ls);
}

